#include "model/eyes_model.h"

void EyesModel::init() {
	emotion_handler_sub_ = nh_.subscribe(Topic::EMO_HANDLER_EYES, 1, &EyesModel::handlerCallback, this);

	joint_state_.name.resize(4);
	joint_state_.position.resize(4);

	joint_state_.name[0] = Joint::LEFTEYEPITCH;
	joint_state_.name[1] = Joint::LEFTEYEYAW;
	joint_state_.name[2] = Joint::RIGHTEYEPITCH;
	joint_state_.name[3] = Joint::RIGHTEYEYAW;
}

void EyesModel::spin() {
	ros::Rate loop_rate(30);
	setInPosition(false);

	while (ros::ok()) {
		if (!isInPosition()) {
			moveEyes();
		}

		prepState();
		joint_pub_.publish(joint_state_);
		ros::spinOnce();
		loop_rate.sleep();
	}

}

void EyesModel::handlerCallback(const comm::eyes::ConstPtr &position) {
	lp_	= position->leftpitch * -1;
	ly_	= position->leftyaw;
	rp_	= position->rightpitch * -1;
	ry_	= position->rightyaw;
}

void EyesModel::moveEyes() {
	// Use this for smoothing out the eyes.
	if (lp_ > ModelValues::EYES_MAX_PITCH) {
		leftpitch_ = ModelValues::EYES_MAX_PITCH;
	} else if (lp_ < ModelValues::EYES_MIN_PITCH) {
		leftpitch_ = ModelValues::EYES_MIN_PITCH;
	} else {
		leftpitch_ = lp_;
	}

	if (rp_ > ModelValues::EYES_MAX_PITCH) {
		rightpitch_ = ModelValues::EYES_MAX_PITCH;
	} else if (rp_ < ModelValues::EYES_MIN_PITCH) {
		rightpitch_ = ModelValues::EYES_MIN_PITCH;
	} else {
		rightpitch_ = rp_;
	}

	if (ly_ > ModelValues::EYES_MAX_YAW) {
		leftyaw_ = ModelValues::EYES_MAX_YAW;
	} else if (ly_ < ModelValues::EYES_MIN_YAW) {
		leftyaw_ = ModelValues::EYES_MIN_YAW;
	} else {
		leftyaw_ = ly_;
	}

	if (ry_ > ModelValues::EYES_MAX_YAW) {
		rightyaw_ = ModelValues::EYES_MAX_YAW;
	} else if (ry_ < ModelValues::EYES_MIN_YAW) {
		rightyaw_ = ModelValues::EYES_MIN_YAW;
	} else {
		rightyaw_ = ry_;
	}
}

void EyesModel::prepState() {
	joint_state_.header.stamp = ros::Time::now();
	joint_state_.position[0] = leftpitch_;
	joint_state_.position[1] = leftyaw_;
	joint_state_.position[2] = rightpitch_;
	joint_state_.position[3] = rightyaw_;
}

int main(int argc, char** argv) {
	ros::init(argc, argv, "eyes_model_publisher");

	EyesModel eyes;
	eyes.init();
	eyes.spin();
}
